#include "parafluidsmoothingkernel.h"
#include "parafluidsphposition.h"
#include "parafluidsphneighbor.h"

#include <QtDebug>


using namespace ParaFluid;

PositionUpdater::~PositionUpdater()
{
	/* does nothing */
}

PositionUpdater::PositionUpdater()
	: AtomVoxelizer::ReadWriteVisitor()
	, _SPH(0)
{
	/* does nothing */
}

void PositionUpdater::updatePosition(SPH & sph)
{
	_SPH = &sph;
	_SPH->atomVoxelizer.acceptVisitor(*this);
}

void PositionUpdater::updatePositionParallel(SPH & sph, int indexFirst, int indexLast)
{
	_SPH = &sph;
	_SPH->atomVoxelizer.acceptVisitor(*this, indexFirst, indexLast);
}

void applyConstraint(float & nextPosition, float & currentPosition, const float & constraintMin, float constraintMax)
{
	if(nextPosition < constraintMin)
	{
		if(constraintMin < currentPosition)
		{
			float temp = currentPosition;
			currentPosition = nextPosition;
			nextPosition = temp;
		}
		else
		{
			nextPosition = constraintMin;
		}
	}
	if(nextPosition > constraintMax)
	{
		if(constraintMax > currentPosition)
		{
			float temp = currentPosition;
			currentPosition = nextPosition;
			nextPosition = temp;
		}
		else
		{
			nextPosition = constraintMax;
		}
	}
}

void PositionUpdater::visit(Atom & atom)
{
	// Apply Integrator
	Vector acceleration = _SPH->gravity();
	float timeStepPow2 = _SPH->timeStep() * _SPH->timeStep();
	if(atom.currentDensity != 0)
	{
		acceleration += atom.currentForce * (1.0 / atom.currentDensity);
	}
	
	atom.nextPosition.x = (2.0 * atom.currentPosition.x) - atom.previousPosition.x + (acceleration.x * timeStepPow2);
	atom.nextPosition.y = (2.0 * atom.currentPosition.y) - atom.previousPosition.y + (acceleration.y * timeStepPow2);
	atom.nextPosition.z = (2.0 * atom.currentPosition.z) - atom.previousPosition.z + (acceleration.z * timeStepPow2);
	
	// Apply Constraints
	applyConstraint(atom.nextPosition.x, atom.currentPosition.x, _SPH->constraints().position.x, _SPH->constraints().position.x + _SPH->constraints().extent.x);
	applyConstraint(atom.nextPosition.y, atom.currentPosition.y, _SPH->constraints().position.y, _SPH->constraints().position.y + _SPH->constraints().extent.y);
	applyConstraint(atom.nextPosition.z, atom.currentPosition.z, _SPH->constraints().position.z, _SPH->constraints().position.z + _SPH->constraints().extent.z);
	
	atom.previousPosition = atom.currentPosition;
	atom.currentPosition = atom.nextPosition;
}
